COMMENT ⊗   VALID 00011 PAGES 
RECORD PAGE   DESCRIPTION
 00001 00001
 00002 00002	LINE - "hardware"-type routines for edges, lines, vertices.
 00005 00003	_ SUMS
 00008 00004	_ MALIN
 00012 00005	_  LINFIT
 00015 00006	_ LINFIT cont
 00017 00007	_ LINFIT cont, ICON
 00020 00008	_ SORTED
 00032 00009	_ WEIGHV
 00035 00010	_  SKAR1, LEKV
 00039 00011	_ REKOP, INREK
 00042 ENDMK
⊗;
COMMENT LINE - "hardware"-type routines for edges, lines, vertices.;

ENTRY SUMS,MALIN,LINFIT,ICON,SORTED,WEIGHV,SKAR1,LEKV,REKOP,INREK;
BEGIN "LINE"
DEFINE QI="INTEGER",
	QR="REAL",
	QRI="REFERENCE INTEGER",
	QRR="REFERENCE REAL",
	QEP="EXTERNAL SIMPLE PROCEDURE",
	QEIP="EXTERNAL SIMPLE INTEGER PROCEDURE",
	QERP="EXTERNAL SIMPLE REAL PROCEDURE",
	QFOP="FORWARD INTERNAL SIMPLE PROCEDURE",
	QFOIP="FORWARD INTERNAL SIMPLE INTEGER PROCEDURE",
	QFORP="FORWARD INTERNAL SIMPLE REAL PROCEDURE",
	_="COMMENT",
	LOOP(I,J,K,L)="FOR I←J STEP L UNTIL K DO",
	SAFEX="SAFE";

REAL A1;
EXTERNAL INTEGER NOEPA,NOL,NOV,IFREEL,NOBAL,LDATE,ILLL,ILFL;
EXTERNAL REAL RDEP,RMEDA,SHRINK,RDDP,RDNP,RMSD,RMLG;
INTERNAL REAL A11, A12, A21, A22, X00, Y00, SX, SY, CX, CY, CL;
SAFEX EXTERNAL INTEGER ARRAY LE,LEDG1,LEDG2,LCREDE[1:1];
SAFEX EXTERNAL REAL ARRAY EAX,EAY,EBX,EBY,XLCOR,YLCOR,CXL,CYL,
	CCL,RLEN,ANGARG[1:1];

	QEP TELL(STRING S);
	QEP UNTELL;
	QEIP KARN(QR X1,Y1,X2,Y2,X3,Y3,X4,Y4; QRR X,Y; QRI IX1,IX2,IP1,IP2;
		  QRR R1,R2; QI IC);
	QEP KOT(QR X1,Y1,X2,Y2,X3,Y3);
	QFOIP ICON(QI I,J,K);
	QEIP  LVNEXT(QI I,J);
	QEP RETLIN(QI I);
	QEIP MSCVCO(QI I,J,K);
	QFOP LEKV(QR X1,Y1,X2,Y2; QRR A,B,C);
	QERP SQRT(QR R);
	QERP COS(QR R);
	QERP ATAN2(QR R,S);
	QERP SIGN(QR R,S);
	QERP AMOD(QR R,S);
	QERP ANGDIR(QR R,S);
	QEIP FAISUM;
	QEP SORLOD;
	QEP SORINT(QRR A,B,C,D; QRI F,E);
	QEP SORBOD(QR A,B,C,D,E,F);
	QEP ANGLIN(QI LL);
	QERP DNEW(QI I; QR X,Y,L);
	QEP WEIFAI(QI LL);
	QEP ANGLEN(QI LL);

_ SUMS;

_	Adds one or more edge-pairs in the various sums
	for the least square fit.
	Returns new mean square deviation, if tolerable,
	else -100.;

INTERNAL SIMPLE REAL PROCEDURE SUMS(INTEGER ILO,IHI1;
		REFERENCE REAL CX,CY,CC; REAL TOL);
	BEGIN "SUMS"
	LABEL L10,L100,L40,L51,L4,L50,L52;
	INTERNAL INTEGER IHI,IHI2;
	REAL RRET,CN,F,G,S1,S2,S3,S4,ROT,SQ1,SQ2,SQT;
	INTERNAL REAL SX2,SY2,SXY;
	RRET←-100.;
	IF IHI1>0 THEN GO L10;
	IHI←ILO;
	IHI2←-IHI1;
	GO L100;

L10:	IHI←IHI1;
L100:	IF ¬FAISUM THEN RETURN(0.0);
	CN←2*(ABS(IHI-ILO)+1);
	F←SX*SY-CN*SXY;
	G←SY*SY-SX*SX+CN*(SX2-SY2);
	IF ABS G <500.* ABS F  THEN GO L4;
	IF ABS CY > ABS CX  THEN GO L40;
	S1←1.;
	S2←0.;
	S3←-SX/CN;
	S4←SX2+S3*SX;
	GO L51;

L40:	S1←0.;
	S2←1.;
	S3←-SY/CN;
	S4←SY2+S3*SY;
L51:	IF S4>CN*TOL THEN RETURN(RRET);
	RRET←S4/CN;
	CX←S1;
	CY←S2;
	CC←S3;
	GO L52;

L4:	G←0.5*G/F;
	ROT←SQRT(G*G+1.);
	S1←1.;
	S2←G+ROT;
	S3←G-ROT;
	SQ1←1.+S2*S2;
	SQ2←1.+S3*S3;
	G←CX+CY*S2;
	SQT←CX+CY*S3;
	IF G*G*SQ2≥SQT*SQT*SQ1 THEN GO L50;
	S2←S3;
	SQ1←SQ2;
L50:	S3←-(SX+SY*S2)/CN;
	S4←(SX2+S2*(S2*SY2+2.*SXY+S3*SY)+S3*SX)/SQ1;
	IF S4>CN*TOL THEN RETURN(RRET);
	SQT←1./SQRT(SQ1);
	CX←S1*SQT;
	CY←S2*SQT;
	CC←S3*SQT;
	RRET←S4/CN;
L52:	IF ABS CC <0.001 THEN CC←0.;
	RETURN(RRET);
	END "SUMS";
_ MALIN;

_	Stores various line-characteristics in the initial fit.;

INTERNAL SIMPLE PROCEDURE MALIN(REAL CX,CY,CC; INTEGER LL;
			REAL SQD; INTEGER ILO,IHI);
	BEGIN "MALIN"
	LABEL L102,L104,L103;
	INTEGER IV1,IV2;
	IV2←2*LL;
	IV1←IV2-1;
	IF CX≠0 THEN GO L102;
	XLCOR[IV1]←EAX[ILO];
	YLCOR[IV1]←-CC;
	XLCOR[IV2]←EBX[IHI];
	YLCOR[IV2]←-CC;
	GO L103;

L102:	IF CY≠0 THEN GO L104;
	XLCOR[IV1]←-CC;
	YLCOR[IV1]←EAY[ILO];
	XLCOR[IV2]←-CC;
	YLCOR[IV2]←EBY[IHI];
	GO L103;

L104:	YLCOR[IV1]←CX*CX*EAY[ILO]-CY*(CX*EAX[ILO]+CC);
	XLCOR[IV1]←-(CY*YLCOR[IV1]+CC)/CX;
	YLCOR[IV2]←CX*CX*EBY[IHI]-CY*(CX*EBX[IHI]+CC);
	XLCOR[IV2]←-(CY*YLCOR[IV2]+CC)/CX;
L103:	CXL[LL]←CX;
	CYL[LL]←CY;
	CCL[LL]←CC;
	ANGLEN(LL);
	LEDG1[LL]←ILO;
	LEDG2[LL]←IHI;
	RETURN;
	END "MALIN";
_  LINFIT;
_	Responsible for the initial line-fit. Fits lines in as
	long successive segments as possible. Sets LCREDE ← LDATE.
	Deletes short lines, based on ILFL and RMLG. Initializes vertices.
	Returns 0 when everything is OK, otherwise the number of
	the edge-pair, at which the overflow occurred in line-space.
	Storage is assumed initialized, so start line-fit!;

INTERNAL SIMPLE INTEGER PROCEDURE LINFIT;
	BEGIN "LINFIT"
	LABEL L1000,L1010,L1003,L1001,L1,L10,L1002,L100,L101,L11,L110,
		L2,L3,L30,L4,L2001;
	INTEGER IV,ILO,IP,IHI,IRET,IRI,IW,ID,IL,IH,INOL,IV1,IV2,IDUM;
	REAL SQD,SQDD,SHR,SHR1,X,Y,CX,CY,CC,SHRIN;

	ILO←1;
	IP←1;
	IHI←1;
	NOL←1;
	IRET←0;
L1000:	IF ILO>NOEPA THEN GO L2001;
	IF NOL≤NOBAL THEN GO L1010;
	NOBAL←30+NOEPA*NOBAL/ILO;
	RETURN(ILO);

L1010:	IRI←1;
	SQD←SUMS(ILO,IHI,CX,CY,CC,RMSD);
L1003:	IW←IHI;
L1001:	CASE LE[IW] OF BEGIN ; GO L1; GO L2; GO L3; GO L4 END;
L1:	IF IRI>0 THEN GO L11;
L10:	ID←IHI-ILO+1;
	IF ID≥ILLL THEN GO L100;
	ILO←IP+1;
L1002:	IHI←ILO;
	IP←ILO;
	GO L1000;

L100:	MALIN(CX,CY,CC,NOL,SQD,ILO,IHI);

_	Last line is stored. See if it can be fused with the previous
	line (iteratively).;

	IF NOL<2∨¬ICON(LEDG2[NOL-1],ILO,1) THEN GO L101;

_	Yes, fuse last two lines, provided mean square deviation is OK!;

	ILO←LEDG1[NOL-1];
	SQD←SUMS(ILO,-IHI,CX,CY,CC,RMSD);
	IF SQD=-100. THEN GO L101;
	NOL←NOL-1;
	GO L100;
_ LINFIT cont;

L101:	ILO←IHI+1;
	NOL←NOL+1;
	GO L1002;

_ 	Change direction and pick up edges there, if possible.;

L11:	IRI←-1;
L110:	IF ILO=1∨ILO≥IHI∨LE[ILO] LAND 1∨NOL≠1∧LEDG2[NOL-1]=ILO-1 THEN GO L10;
	IL←ILO-1;
	IF DNEW(IL,CX,CY,CC)>RDNP THEN GO L10;
	SQDD←SUMS(IHI,IL,CX,CY,CC,RMSD);
	IF SQDD=-100. THEN GO L10;
	ILO←IL;
	IW←ILO;
	SQD←SQDD;
	GO L1001;

L2:	IF IRI≤0 THEN GO L110 ELSE GO L11;
L3:	IF IRI≤0 THEN GO L10;
L30:	IF IHI=NOEPA THEN GO L11;
	IH←IHI+1;
	IF DNEW(IH,CX,CY,CC)>RDNP THEN GO L11;
	SQDD←SUMS(ILO,IH,CX,CY,CC,RMSD);
	IF SQDD=-100. THEN GO L11;
	IHI←IH;
	SQD←SQDD;
	GO L1003;

L4:	IF IRI≤0 THEN GO L110 ELSE GO L30;
L2001:	IFREEL←NOL;
	NOL←NOL-1;

_	The line-fit is now completed. Date the lines (in LCREDE).;

	LOOP(IL,1,NOL,1) LCREDE[IL]←LDATE;
	IFREEL←NOL+1;

_	Now (!) delete short lines, based on ILFL and RMLG, and initialize
	vertices at ends of good lines, and shorten such lines by
	SHRINK at each end, to minimize overshoot.;

	INOL←NOL;
	NOV←0;
	SHRIN←1.5 MAX (SHRINK*RDEP);
	LOOP(IL,1,INOL,1)
	IF LEDG2[IL]-LEDG1[IL]+1<ILFL∨RLEN[IL]<RMLG+0.*SHRIN THEN RETLIN(IL)
		ELSE BEGIN "LPU1"
_ LINFIT cont, ICON;

_	The line is good. Initialize its end-vertices,
	and shrink the line.;

		SHR1←SHRIN MIN (0.25*RLEN[IL]);
		SHR←SHR1/RLEN[IL];
		RLEN[IL]←RLEN[IL]-2.0*SHR1;
		IV2←2*IL;
		IV1←IV2-1;
		X←XLCOR[IV1];
		Y←YLCOR[IV1];
		XLCOR[IV1]←X+SHR*(XLCOR[IV2]-X);
		YLCOR[IV1]←Y+SHR*(YLCOR[IV2]-Y);
		XLCOR[IV2]←XLCOR[IV2]+SHR*(X-XLCOR[IV2]);
		YLCOR[IV2]←YLCOR[IV2]+SHR*(Y-YLCOR[IV2]);
		LOOP(IV,IV1,IV2,1) IDUM←MSCVCO(IV,0,1);
	        END "LPU1";
	RETURN(IRET);
	END "LINFIT";

_	Finds out whether two edge-pairs are in the same connected set,
	and at a distance ≤ IDIS (i.e. number of edge-pairs in between).;

INTERNAL SIMPLE INTEGER PROCEDURE ICON(INTEGER L12,L21,IDIS);
	BEGIN "ICON"
	INTEGER I3;
	IF L21-L12-1>IDIS∨LE[L12]<3∨LE[L21]≠2∧LE[L21]≠4 THEN RETURN(0);
	IF L21-L12>1 THEN LOOP(I3,L12+1,L21-1,1) IF LE[I3]≠4 THEN RETURN(0);
	RETURN(1);
	END "ICON";
_ SORTED;

_	This program sorts edge-pairs into connected subsets.;

INTERNAL PROCEDURE SORTED;
	BEGIN "SORTED"
	REAL GRAV,RDEP2,DDQ,FAK,A1,A2;
	SAFEX REAL ARRAY FAX,FAY,FBX,FBY[1:NOEPA];
	SAFEX INTEGER ARRAY IFO,IBA[1:NOEPA];
	SORINT(FAX[1],FAY[1],FBX[1],FBY[1],IFO[1],IBA[1]);

	TELL("edge-sorting");

_	Save edge-arrays and zero LE.
	Set up center coordinates in EAX and EAY.
	EBX will later contain the IBA-distance, EBY the IFO-distance.;

	ARRBLT(FAX[1],EAX[1],NOEPA);
	ARRBLT(FAY[1],EAY[1],NOEPA);
	ARRBLT(FBX[1],EBX[1],NOEPA);
	ARRBLT(FBY[1],EBY[1],NOEPA);
	SORLOD;

_	Establish backward and forward links.;

	GRAV←RDEP*(2.01+RDDP);
 	RDEP2←4.*RDEP↑2;
	DDQ←RDEP2*RDDP↑2;
	FAK←RDDP↑2/((15.1/(16.0*COS(RMEDA/114.58)↑4-0.9))↑2-1.0);
	A1←RDEP2↑2/16.0;
	A2←0.9*A1;
	A1←15.1*A1;
	SORBOD(GRAV,RDEP2,DDQ,FAK,A1,A2);
	UNTELL;
	END "SORTED";
_ WEIGHV;

_	Computes the weighted least-square coordinates, (x,y), and the
	weight ( ← sum of square-roots of lengths of lines involved),
	for the c.v. ICV, also counting inactive lines iff ICV<0.;

INTERNAL SIMPLE PROCEDURE WEIGHV(INTEGER ICV; REFERENCE REAL X,Y,WE);
	BEGIN "WEIGHV"
	INTEGER ISV,LL,IND;
	REAL SA2,SB2,SAB,SAC,SBC,DS;
	INTERNAL REAL W;
	IND←1;
	IF ¬(ISV←ABS LVNEXT(ICV,9)) THEN RETURN;
	SA2←SB2←SAB←SAC←SBC←SX←SY←WE←0.;

_	OK, that was the initialization part. Now do it!;

	DO	BEGIN "WEI"
		WEIFAI(ISV);
		WE←WE+W;
		SA2←SA2+W*CX↑2;
		SB2←SB2+W*CY↑2;
		SAB←SAB+W*CX*CY;
		SAC←SAC+W*CX*CL;
		SBC←SBC+W*CY*CL;
		END "WEI" UNTIL (ISV←ABS LVNEXT(0,9))≤0;

_	The various sums are now computed. Find coordinates.;

	DS←SAB*SAB-SA2*SB2;

_	DS is a measure of the covariance of x- and y-coordinates, so that
	DS tends to be small for collinear lines. Hence the special case.;

	IF ABS DS ≤ 0.1 THEN 
		BEGIN
		X← SX/WE;
		Y←SY/WE;
		END ELSE BEGIN
		Y←(SA2*SBC-SAC*SAB)/DS;
		X←-(SAC+Y*SAB)/SA2;
		END;
	END "WEIGHV";
_  SKAR1, LEKV;

_	Finds intersection of (X1,Y1)-(X2,Y2) with line LL, after 
	(X1,Y1) on the ray only. Returns point of intersection, (X,Y),
	and distance from (X1,Y1), RSQ.
	RSQ ← 900000. iff there is no such intersection.;

INTERNAL SIMPLE PROCEDURE SKAR1(REAL X1,Y1,X2,Y2; INTEGER LL; 
	        REFERENCE REAL X,Y,RSQ);
	BEGIN "SKAR1"
	INTEGER IV,IDUM,I1,I2,I3,I4;
	REAL R2;
	IV←2*LL;
	IDUM←KARN(X1,Y1,X2,Y2,XLCOR[IV-1],YLCOR[IV-1],
		  XLCOR[IV],YLCOR[IV],X,Y,I1,I2,I3,I4,RSQ,R2,1);
	RSQ←(X1-X)↑2+(Y1-Y)↑2;
	IF I3=0∨I3=1∨I3=-1∧RSQ<0.25 THEN RSQ←900000.;
	RETURN;
	END "SKAR1";


_	Finds the normalized line-equation.;

INTERNAL SIMPLE PROCEDURE LEKV(REAL X1,Y1,X2,Y2; REFERENCE REAL A,B,C);
	BEGIN "LEKV"
	REAL R;
	IF ABS(Y1-Y2)≤0.01 THEN BEGIN A←0.;B←1.;C←-0.5*(Y1+Y2);RETURN END;
	A←1.;
	B←(X2-X1)/(Y1-Y2);
	R←SQRT(A↑2+B↑2);
	IF ABS (A←A/R) < 0.002  THEN A←0.;
	IF ABS (B←B/R) < 0.002 THEN B←0.;
	IF ABS (C←-A*X1-B*Y1) < 0.001 THEN C←0.
	END "LEKV";
_ REKOP, INREK;

_	Sets up transformation data from internal representation into
	a rectangular operator with the line (X1,Y1)-(X2,Y2) as axis,
	and of width RR. RL returns ratio of length of rectangle to
	width of rectangle. The origin of this new coordinate system
	is positioned at the centre of gravity of the rectangle.
	The new X-axis is directed to (X2,Y2), and the Y-axis goes
	out perpendicularly on the left.;

INTERNAL SIMPLE PROCEDURE REKOP(REAL X1,Y1,X2,Y2,RR; REFERENCE REAL RL);
	BEGIN "REKOP"
	REAL DX2,DY2,DX3,DY3,Q2,Q3,X,Y,X3,Y3;
	X←0.5*(X1+X2);
	Y←0.5*(Y1+Y2);
	RL←SQRT((X1-X2)↑2+(Y1-Y2)↑2)/RR;
	X3 ← X-(Y2-Y)/RL;
	Y3 ← Y+(X2-X)/RL;
	DY2←ABS(DX2←Y2-Y) MAX 0.000001;
	DY3←ABS(DX3←Y3-Y) MAX 0.000001;
	IF DX2<0 THEN DY2 ← -DY2;
	IF DX3<0 THEN DY3←-DY3;
	DX2←X2-X;
	DX3←X3-X;
	Q2←DX2/DY2;
	Q3←DX3/DY3;
	A11←1./(DX2-Q3*DY2);
	A21←1./(DX3-Q2*DY3);

_	Note that e.g. DX2-Q3*DY2=0 would imply parallelity of the axes.
	Also e.g. DX2 and DX3 cannot simultaneously be 0.;

	IF ABS(Q3)<.000001 THEN Q3←IF Q3≥0 THEN .000001 ELSE -.000001;
	IF ABS(Q2)<.000001 THEN Q2←IF Q2≥0 THEN .000001 ELSE -.000001;
	A12←1./(DY2-DX2/Q3);
	A22←1./(DY3-DX3/Q2);
	X00←X;
	Y00←Y
	END "REKOP";


_	Returns T (else F) iff (X,Y) is inside current rectangular operator.;

INTERNAL SIMPLE INTEGER PROCEDURE INREK(REAL X,Y);
	RETURN(ABS(A11*(X-X00)+A12*(Y-Y00))≤1.∧
		ABS(A21*(X-X00)+A22*(Y-Y00))≤1.);
END "LINE";